/**
 kint.zhao  huasheng_zyh@163.com
   2017.0721
*/
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <assert.h>



#include "arcsoft_fsdk_face_detection.h"

#include "merror.h"
#include "face.h"
#include <opencv2/opencv.hpp>

#define APPID     "3ZdMTGTqLYiW2wce6foyGCbLUzA6chF2jeJyybahnGQP"
#define SDKKEY    "2faJKE5Jdj3YDmidqPtmoET7X2hWsgbJewUxa9HWLpGR"


#define INPUT_IMAGE_FORMAT  ASVL_PAF_RGB24_B8G8R8
#define WORKBUF_SIZE        (40*1024*1024)
#define MAX_FACE_NUM        (50)

std::string getHighestScoreUserName(Json::Value &root)
{
    std::string name;
    float highest_score = 0;

    if (!root["error_code"].isNull())
        return name;

    int result_num = root["result_num"].asInt();
    Json::Value result = root["result"];
    for (int i = 0; i < result_num; i++) {
        int score = result[i]["scores"][0].asInt();
        if (score > highest_score) {
            name = result[i]["user_info"].asString();
            highest_score = score;
        }
    }

    return name;
}

int main(int argc, char* argv[]) {
    // 设置APPID/AK/SK
    std::string app_id = "9946091";
    std::string api_key = "0dBydMccOMLRUq7SxTl5SWxk";
    std::string secret_key = "aATCgLXO1c6rsS2rOmEYSj2tAm9bP9wq";
    Json::Value result;

    std::string group_id = "group1";

    aip::Face client(app_id, api_key, secret_key);

    //初始化节点 
    ros::init(argc, argv, "face_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 

    CvCapture* cam0 = cvCaptureFromCAM(0);
    if(!cam0) {
        fprintf(stderr, "Could not initialize opening of Camera 0..\n");
        system("Pause");
        return -1;
    }

    ros::Publisher hand_pub = nh.advertise<std_msgs::String>("handup", 10);
    ros::Publisher hand_free_pub = nh.advertise<std_msgs::String>("handfree", 10);
    ros::Publisher tts_pub = nh.advertise<std_msgs::String>("tts", 10);

    // cvReleaseCapture(&cam0);

    MByte *pWorkMem = (MByte *)malloc(WORKBUF_SIZE);
    if(pWorkMem == nullptr){
        fprintf(stderr, "fail to malloc workbuf\r\n");
        exit(0);
    }

    MHandle hEngine = nullptr;
    int ret = AFD_FSDK_InitialFaceEngine(APPID, SDKKEY, pWorkMem, WORKBUF_SIZE, 
                                         &hEngine, AFD_FSDK_OPF_0_HIGHER_EXT, 16, MAX_FACE_NUM);
    if (ret != 0) {
        fprintf(stderr, "fail to AFD_FSDK_InitialFaceEngine(): 0x%x\r\n", ret);
        free(pWorkMem);
        exit(0);
    }

    // cvNamedWindow("Camera 0",CV_WINDOW_AUTOSIZE); //create a window called "Camera 0"

    bool detec_status = false;
    float array[10] = {0,0,0,0,0,0,0,0,0,0};
    int pos = 0;

    //指定循环的频率 
    ros::Rate loop_rate(60); 
    while(ros::ok()) {

        IplImage *cam0Frame = cvQueryFrame(cam0);
        ASVLOFFSCREEN inputImg = { 0 };
        inputImg.u32PixelArrayFormat = INPUT_IMAGE_FORMAT;
        inputImg.i32Width = cam0Frame->width;
        inputImg.i32Height = cam0Frame->height;
        inputImg.ppu8Plane[0] = (MUInt8*)&cam0Frame->imageData[0];
        inputImg.pi32Pitch[0] = inputImg.i32Width*3;

        LPAFD_FSDK_FACERES faceResult;
        ret = AFD_FSDK_StillImageFaceDetection(hEngine, &inputImg, &faceResult);
        if (ret != 0) {
            fprintf(stderr, "fail to AFD_FSDK_StillImageFaceDetection(): 0x%x\r\n", ret);
            free(inputImg.ppu8Plane[0]);
            AFD_FSDK_UninitialFaceEngine(hEngine);
            free(pWorkMem);
            exit(0);
        }
        if (faceResult->nFace > 0)
            array[pos] = 1.0;
        else
            array[pos] = 0.0;
        if (pos >= sizeof(array))
            pos = 0;

        pos += 1;
        float ava = 0.0;
        fprintf(stderr, "sizeof(array) = : %d\r\n", sizeof(array));
        for (int i = 0; i < 10; i++) {
            ava += array[i];
        }

        fprintf(stderr, "ava: %f\r\n", ava);
        fprintf(stderr, "fail to AFD_FSDK_StillImageFaceDetection(): %f\r\n", ava / sizeof(array));
        if (ava / 10 > 0.5) {
            if (detec_status == false) {
                std_msgs::String result;
                hand_pub.publish(result);
                detec_status = true;
                for (int i = 0; i < faceResult->nFace; i++) {
                    CvMat *pMat = cvGetSubRect(cam0Frame, cvCreateMatHeader(100, 100, CV_8UC1), cvRect(faceResult->rcFace[i].left, faceResult->rcFace[i].top, faceResult->rcFace[i].right - faceResult->rcFace[i].left, faceResult->rcFace[i].bottom - faceResult->rcFace[i].top));
                    IplImage *subImg = cvGetImage(pMat, cvCreateImageHeader(cvSize(100, 100), 8, 1));
                    cvSaveImage("1.jpg", subImg);
                    std::string image;
                    aip::get_file_content("1.jpg", &image);
                    Json::Value identify_result;
                    identify_result = client.identify(group_id, image, aip::null);
                    std::string name = getHighestScoreUserName(identify_result);
                    if (!name.empty()) {
                        std_msgs::String result;
                        result.data = "你好啊亲爱的" + name;
                        tts_pub.publish(result);
                        
                        // CvFont font;
                    
                        // cvInitFont(&font, cv::FONT_HERSHEY_COMPLEX, 2, 2);
                        // // cv::putText(cam0Frame, text, Point(faceResult->rcFace[i].left, faceResult->rcFace[i].top), font_face, font_scale, cv::Scalar(0, 255, 255), thickness, 8, 0);  
                        // cvPutText(cam0Frame, name.c_str(), cvPoint(faceResult->rcFace[i].left, faceResult->rcFace[i].top), &font, cvScalar(255, 0, 0));            
                        // printf("%s", name.c_str());
                    }
                }
        
            }
        } else {
            if (detec_status == true) {
                std_msgs::String result;
                hand_free_pub.publish(result);
                detec_status = false;
            }
            detec_status = false;
        }
        //         if detec_status is False:
        //             pub_tts.publish("你好")
        //             detec_status = True
        //     else:
        //         detec_status = False

        // for (int i = 0; i < faceResult->nFace; i++) {
        //     printf("face %d:(%d,%d,%d,%d)\r\n", i, 
        //         faceResult->rcFace[i].left, faceResult->rcFace[i].top,
        //         faceResult->rcFace[i].right, faceResult->rcFace[i].bottom);
            
        // }

        ros::spinOnce(); 
        loop_rate.sleep();
    }
    // free(inputImg.ppu8Plane[0]);

    cvReleaseCapture(&cam0);
	AFD_FSDK_UninitialFaceEngine(hEngine);
    free(pWorkMem);

    return 0;
}
